/*
 * Calibrator.java
 *
 * Created on June 12, 2007, 9:48 PM
 *
 * Calibrator acts before actuation or sensation loops are started to correct 
 * errors in the robot's turning by establish what is a correct turn via human 
 * intervention. 
 * It will cause the bot to turn what it thinks is 90 degrees, then wait for the
 * operator to establish what is really 90, and save that value.
 */

package org.gunncs.daemons;

import com.iaroc.irobot.IRobotCreate;
import com.sun.spot.sensorboard.EDemoBoard;
import com.sun.spot.sensorboard.peripheral.ISwitch;
import com.sun.spot.sensorboard.peripheral.ISwitchListener;

/**
 *
 * @author Anandg
 */
public class Calibrator implements ISwitchListener{
    IRobotCreate bot;
    EDemoBoard board;
    boolean calibrated;
    double offset;
    /** Creates a new instance of Calibrator */
    public Calibrator(IRobotCreate rawbot) {
        bot=rawbot;
        board=EDemoBoard.getInstance();
        calibrated=false;
        offset=0.0;
    }
    
    public void beginCalibration(){
        board.getSwitches()[0].addISwitchListener(this);
        board.getSwitches()[1].addISwitchListener(this);
        bot.spinRight();
        bot.waitAngle(90);
        bot.stop();
    }
    
    public void endCalibration(){
        board.getSwitches()[0].removeISwitchListener(this);
        board.getSwitches()[1].removeISwitchListener(this);
        calibrated=true;
    }
    
    public double getOffset(){
        return offset;  //Turn offset in degrees adjustment per 90
    }
    
    public void switchPressed(ISwitch sw) {
        if(board.getSwitches()[0].isClosed() && board.getSwitches()[1].isClosed()){
            endCalibration();
        }
    }

    public void switchReleased(ISwitch sw) {
        if(sw.equals(board.getSwitches()[0])){
            bot.spinLeft();
            bot.waitAngle(1);
            bot.stop();
            offset-=1;
        }else if(sw.equals(board.getSwitches()[1])){
            bot.spinRight();
            bot.waitAngle(1);
            bot.stop();
            offset+=1;
        }
    }
    
    public boolean isCalibrated(){
        return calibrated;
    }
    
    
    
}
